#ifndef _SERVO_NEWSTART_H__
#define _SERVO_NEWSTART_H__

#include <motor/motor.h>

#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/float64.hpp>

namespace ros_canopen_driver {

struct NEWSTART_PDO_CACHE
{ 
    std::atomic<uint8_t>     mode{0};
    std::atomic<uint16_t>    error_code{0};
    std::atomic<int32_t>     current_pos{0};
    std::atomic<int32_t>     current_vel{0};
    std::atomic<uint16_t>    status_word{0};
    std::atomic<uint32_t>    digital_input{0};
    std::atomic<uint32_t>    profile_vel{0};
    std::atomic<int32_t>     target_pos{0};
    std::atomic<int16_t>     torque{0};
    std::atomic<double>      timestamp{0.0};
};

class ServoNewstart : public Motor
{
public:
    ServoNewstart(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoNewstart();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_HomeMachine();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_SwitchMode(int8_t mode);

 
    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);

    void Motor_SetTargetPosEnc(int pos);

    void Motor_ForkLift(bool up, int vel);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}

    std::string Motor_Err2userErr(const uint16_t& motorErr);

private:
    // Canopen *pCanopen;
    SERVOMOTOR_PORT m_servo_motor;
    int preTargetEnc;
    std::atomic<MotorState> motorState;

    struct NEWSTART_PDO_CACHE pc_;

    int pos_send_scale;

    double rotary_pos;
    double rotary_old_pos;
    double rotary_delta_pos;

    // rpdo pub
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>  set_vel_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>  set_pos_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_control_word_pub;
    // sdo pub
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_control_word_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_di_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>>  set_home_offset_mode_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_homing_switch_speed_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_homing_zero_speed_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>  set_home_offset_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_homing_acc_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_homing_method_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int8>>  set_mode_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_acc_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_dec_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>> set_p_vel_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>  set_pos_sdo_pub;

    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr     mode_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr    errcode_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     current_pos_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     current_vel_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr    status_word_sub;
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr    di_sub;
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr    p_vel_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr     target_pos_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr   timestamp_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr     torque_sub;
    
    void mode_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);
    void errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void current_vel_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void status_word_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void di_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg);
    void p_vel_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg);
    void target_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg);
    void torque_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);

    void servo_modeSet(uint8_t mode);

    void servo_homeOffsetModeSet(uint8_t mode);

    void servo_homeOffsetSet(int homeOffset);

    void servo_homeMethodSet(int homeMethod);

    void servo_homeSpeedHSet(uint32_t homeSpeedH);

    void servo_homeSpeedLSet(uint32_t homeSpeedL);

    void servo_homeAccSet(uint32_t homeAcc);

    void servo_homeStart();

    int servo_encoderGet();

    void servo_PRelMove(int data);

    int servo_velGet();

    uint32_t servo_PvelGet();

    int servo_VvelGet();

    uint32_t servo_DiStatusGet();

    void servo_readDiStatus();

    void servo_ErrClear();

};

}
#endif
